#include "os_cpu.h"
#include "os_cfg.h"
#include "qos.h"

static os_tcb_t os_tcb_table[QOS_MAX_TASKS];
static bool os_running = false;
static int32u_t os_time = 0;
static int16u_t os_prio_curr = TASK_INVALID_PRIO;


// task management----------------------
static void qos_task_default(void){
    // This function will never be called.
    qos_verify( false );
}





static void qos_task_highest_prio(void){
}





static void qos_task_idel(void){
}





static void qos_tcb_init_idel(void){
    os_tcb_t * const tcb = &os_tcb_table[ TASK_IDEL_PRIO ];

    tcb->prio = QOS_MAX_TASKS-1;
    tcb->state = TASK_STATE_READY;
    tcb->delay_tick = 0;
    tcb->task = qos_task_idel;
}





static void qos_tcb_init_highest_prio(void){
    os_tcb_t * const tcb = &os_tcb_table[TASK_HIGHEST_PRIO];

    tcb->prio = 0;
    tcb->state = TASK_STATE_DORMANT;
    tcb->delay_tick = 0;
    tcb->task = qos_task_highest_prio;
}





static void qos_tcb_init(void){
    int16u_t i = 0;
    os_tcb_t *tcb = NULL;

    // at least one idel task & highest prio task for OS
    qos_verify( QOS_MAX_TASKS >= QOS_SYS_TASKS );

    tcb = os_tcb_table;
    // initial tcb, not include idel and highest prio
    for(i=1; i<QOS_MAX_TASKS-1; i++, tcb++){
        tcb->prio = i;
        tcb->state = TASK_STATE_DORMANT;
        tcb->delay_tick = 0;
        tcb->task = qos_task_default;
    }

    qos_tcb_init_idel();
    qos_tcb_init_highest_prio();
}




int16u_t highest_ready_task_prio(void){
    int16u_t i=0;
    os_tcb_t const *tcb = os_tcb_table;

    // go through all TCBs in TCB list
    for(i=0; i<QOS_MAX_TASKS; i++, tcb++){
        qos_verify( TASK_STATE_RUN != tcb->state );
        if( TASK_STATE_READY == tcb->state ){
            return i;
        }
    }

    // never run to here, because idel task always ready.
    qos_verify( false );
    return TASK_IDEL_PRIO;
}




static void qos_task_switch_state( int16u_t prio, int8u_t state ){
    os_tcb_t * const tcb = &os_tcb_table[ prio ];

    qos_verify( QOS_MAX_TASKS > prio );
    qos_verify( TASK_STATE_MAX >= state );

    // idel task's state couldn't be TASK_STATE_WAIT
    if( TASK_IDEL_PRIO == prio ){
        qos_verify( TASK_STATE_RUN == state || TASK_STATE_READY == state );
    }

    tcb->state = state;
}






static void qos_task_sched( int16u_t prio ){
    os_tcb_t * const tcb = &os_tcb_table[ prio ];

    qos_verify( QOS_MAX_TASKS > prio );
    qos_verify( TASK_STATE_READY == tcb->state );

    qos_task_switch_state( prio, TASK_STATE_RUN );
    os_prio_curr = tcb->prio;
    (tcb->task)();
    // if not call qos_sleep(), qos_pend(), etc, ..., switch it to READY
    if( TASK_STATE_RUN == tcb->state ){
        qos_task_switch_state( tcb->prio, TASK_STATE_READY );
    }

    return;
}






static void qos_tasks_sched(void){
    int16u_t i=0;
    int16u_t ready_task_prio = 0;

    // get ready task id
    ready_task_prio = highest_ready_task_prio();
    qos_verify( ready_task_prio < QOS_MAX_TASKS );
    qos_task_sched( ready_task_prio );
}





void qos_task_create( int16u_t prio, const task_entry_t task ){
    os_tcb_t * const tcb = &os_tcb_table[ prio ];

    qos_verify( NULL != task );
    // lowest prio task is idel task
    qos_verify( TASK_IDEL_PRIO > prio );
    // highest prio task is system task
    qos_verify( 0 < prio );

    tcb->prio = prio;
    tcb->state = TASK_STATE_READY;
    tcb->delay_tick = 0;
    tcb->task = task;
}




void qos_task_tick( int16u_t prio, int32u_t tick ){
    os_tcb_t * const tcb = &os_tcb_table[ prio ];

    qos_verify( QOS_MAX_TASKS > prio );
    qos_verify( 0 != tick );
    qos_verify( TASK_STATE_MAX >= tcb->state );

    if( TASK_STATE_WAIT != tcb->state ){
        return;
    }

    if( tick >= tcb->delay_tick ){
        tcb->delay_tick = 0;
        qos_task_switch_state( tcb->prio, TASK_STATE_READY );
    }
    else{
        tcb->delay_tick -= tick;
    }

    return;
}




// time management----------------------
void qos_sleep(int16u_t ticks){
    os_tcb_t * const tcb = &os_tcb_table[ os_prio_curr ];

    qos_verify( QOS_MAX_TASKS > os_prio_curr );
    qos_verify( 0 != ticks );

    tcb->delay_tick = ticks;
    qos_task_switch_state( tcb->prio, TASK_STATE_WAIT );
}




void qos_time_tick_isr(void){
    qos_enter_critical();
    ++os_time;
    qos_exit_critical();
}




static void qos_time_tick(void){
    int16u_t i=0;
    int32u_t os_time_store = 0;     // store os_time, for os_time modify in isr

    qos_enter_critical();
    os_time_store = os_time;
    os_time = 0;
    qos_exit_critical();

    // one or more tick pass?
    if( 0 == os_time_store ){
        return;
    }

    // go through all TCBs in TCB list
    for(i=0; i<QOS_MAX_TASKS-1; i++){
        qos_task_tick( i, os_time_store );
    }

    return;
}




// miscellaneous------------------------
void qos_init(void){
    qos_verify( !os_running );
    qos_tcb_init();
    qos_time_tick_init();
}




static void qos_start_once(void){
    qos_time_tick();
    qos_tasks_sched();
}




void qos_start(void){
    qos_verify( !os_running );
    os_running = true;

    while(1){
        qos_start_once();
    }
}

// no more------------------------------
